{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "from local_plot import *\n",
    "from utils import *\n",
    "def read_multi_folder(folder, nodes, enable_pgo=True):\n",
    "    paths = {}\n",
    "    paths_pgo = {}\n",
    "    t0 = None\n",
    "    for i in nodes:\n",
    "        output_folder = folder + str(i) + \"/\"\n",
    "        _paths, t0 = read_paths(output_folder, [i], t0=t0)\n",
    "        if enable_pgo:\n",
    "            _paths_pgo, t0 = read_paths(output_folder, [i], prefix=\"pgo_\", t0=t0)\n",
    "            paths_pgo[i] = _paths_pgo[i]\n",
    "        paths[i] = _paths[i]\n",
    "    if len(paths_pgo) == 0:\n",
    "        return paths, None, t0\n",
    "    return paths, paths_pgo, t0\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 5, 6, 7]\n",
    "output_folder = \"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/outputs/d2vins-5/swarm\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14\"\n",
    "dte = 550\n",
    "paths, paths_pgo, t0 = read_multi_folder(output_folder, nodes, enable_pgo=False)\n",
    "paths_gt, _ = read_paths(data_folder, nodes, prefix=\"eight_noyaw_\", suffix=\"-groundtruth.txt\", t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True)\n",
    "if paths_pgo is not None:\n",
    "    align_paths(paths_pgo, paths_gt, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte))\n",
    "\n",
    "# paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono_noyaw_\", t0=t0)\n",
    "# print(\"VINS-Mono\")\n",
    "# align_paths(paths_vo, paths_gt)\n",
    "# display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "# display(relative_pose_err(nodes, paths_vo, paths_gt, dte=dte))\n",
    "\n",
    "# plot_fused(nodes, paths, poses_gt=paths_gt, figsize=(10,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4, 5]\n",
    "output_folder = \"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/outputs/d2vins-5-yaw-async/swarm\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14\"\n",
    "dte = 550\n",
    "paths, paths_pgo, t0 = read_multi_folder(output_folder, nodes, enable_pgo=False)\n",
    "paths_gt, _ = read_paths(data_folder, nodes, prefix=\"eight_yaw_\", suffix=\"-groundtruth.txt\", t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True)\n",
    "if paths_pgo is not None:\n",
    "    align_paths(paths_pgo, paths_gt, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte))\n",
    "\n",
    "paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono_yaw_\", t0=t0)\n",
    "print(\"VINS-Mono\")\n",
    "align_paths(paths_vo, paths_gt)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths_vo, paths_gt, dte=dte))\n",
    "\n",
    "# plot_fused(nodes, paths, poses_gt=paths_gt, figsize=(10,10))\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4, 5]\n",
    "output_folder = \"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/outputs/d2vins-5-yaw-sync/swarm\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14\"\n",
    "dte = 550\n",
    "paths, paths_pgo, t0 = read_multi_folder(output_folder, nodes, enable_pgo=False)\n",
    "paths_gt, _ = read_paths(data_folder, nodes, prefix=\"eight_yaw_\", suffix=\"-groundtruth.txt\", t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True)\n",
    "if paths_pgo is not None:\n",
    "    align_paths(paths_pgo, paths_gt, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2]\n",
    "output_folder = \"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/outputs/d2slam-2-yaw/swarm\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14\"\n",
    "dte = 550\n",
    "paths, paths_pgo, t0 = read_multi_folder(output_folder, nodes, enable_pgo=True)\n",
    "paths_gt, _ = read_paths(data_folder, nodes, prefix=\"eight_yaw_\", suffix=\"-groundtruth.txt\", t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "if paths_pgo is not None:\n",
    "    align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(1, nodes[1:], paths, paths_gt, dte=dte, show=False))\n",
    "# plot_fused(nodes, paths, poses_pgo=paths_pgo, poses_gt=paths_gt, figsize=(10,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4, 5]\n",
    "output_folder = \"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/outputs/d2slam-5-yaw/swarm\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14\"\n",
    "dte = 550\n",
    "paths, paths_pgo, t0 = read_multi_folder(output_folder, nodes, enable_pgo=True)\n",
    "paths_gt, _ = read_paths(data_folder, nodes, prefix=\"eight_yaw_\", suffix=\"-groundtruth.txt\", t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "if paths_pgo is not None:\n",
    "    align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Find the nearest pair of datasets\n",
    "paths_gt = {}\n",
    "t0 = None\n",
    "nodes = [1, 2, 4, 5, 6, 7]\n",
    "for i in nodes:\n",
    "    paths_gt[i], t0 = read_path_from_csv(f\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/eight_noyaw_{i}-groundtruth.txt\", t0)\n",
    "output_table = [i for i in range(0, 7)]\n",
    "output_table[0] = \"Drone\"\n",
    "\n",
    "dis_sum = 0.0\n",
    "count = 0\n",
    "\n",
    "for i in nodes:\n",
    "    for j in nodes:\n",
    "        if j > i:\n",
    "            t = find_common_times(paths_gt[i].t, paths_gt[j].t)\n",
    "            ate_pos = ATE_POS(paths_gt[i].pos_func(t), paths_gt[j].pos_func(t))\n",
    "            dis_sum += ate_pos\n",
    "            count += 1\n",
    "            print(f\"i={i}, j={j} ate_pos={ate_pos}\")\n",
    "print(f\"Average ate_pos={dis_sum/count}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Find the nearest pair of datasets\n",
    "paths_gt = {}\n",
    "t0 = None\n",
    "nodes = [1, 2, 3, 4, 5]\n",
    "for i in nodes:\n",
    "    paths_gt[i], t0 = read_path_from_csv(f\"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/eight_yaw_{i}-groundtruth.txt\", t0)\n",
    "output_table = [i for i in range(0, 7)]\n",
    "output_table[0] = \"Drone\"\n",
    "\n",
    "dis_sum = 0.0\n",
    "count = 0\n",
    "\n",
    "for i in nodes:\n",
    "    for j in nodes:\n",
    "        if j > i:\n",
    "            t = find_common_times(paths_gt[i].t, paths_gt[j].t)\n",
    "            ate_pos = ATE_POS(paths_gt[i].pos_func(t), paths_gt[j].pos_func(t))\n",
    "            dis_sum += ate_pos\n",
    "            count += 1\n",
    "            print(f\"i={i}, j={j} ate_pos={ate_pos}\")\n",
    "print(f\"Average ate_pos={dis_sum/count}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
